import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import numpy as np
import cv2
import os
from PIL import Image
from PIL import ImageFont
from PIL import ImageDraw
import math
%matplotlib inline
### CAMERA CALIBRATION
nx = 9 # Number of inside corners in x
ny = 6 # Number of inside corners in y
images = []
count = 0
for i in range(20):
image_name = 'camera_cal/calibration' + str(count+1) + '.jpg'
images.append(image_name)
#print (image_name)
count = count + 1
objpoints = []
imgpoints = []
objp = np.zeros((nx*ny, 3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2) # x and y coordinates
f = plt.figure(1)
f.set_figheight(80)
f.set_figwidth(80)
print ('')
count = 0
for fname in images:
print (fname)
img = mpimg.imread(fname)
gray_image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray_image, (nx,ny), None)
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
img = cv2.drawChessboardCorners(img, (nx,ny), corners, ret)
plt.subplot(20,1,count + 1)
count = count + 1
plt.imshow(img)
print("Number of images used for calibration: ", count)
### COLLECT AND SAVE FRAMES FROM VIDEO
def video_to_frames(video, path_output_dir):
# extract frames from a video and save to directory as 'x.png' where
# x is the frame index
pipeline_images = []
vidcap = cv2.VideoCapture(video)
count = 0
while vidcap.isOpened():
success, image = vidcap.read()
if success:
cv2.imwrite(os.path.join(path_output_dir, 'image%d.jpg') % count, image)
count += 1
image_string = 'image%d.jpg' %count
pipeline_images.append(image_string) #list of images in ascending order
else:
break
cv2.destroyAllWindows()
vidcap.release()
return pipeline_images
### CORRECTION OF DISTORTION
def cal_undistort(img, objpoints, imgpoints):
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray_img.shape[::-1], None, None)
undist = cv2.undistort(img, mtx, dist, None, mtx)
return undist, mtx, dist
### GRADIENT THRESHOLDS
# Define a function that thresholds the S-channel of HLS
def hls_select(img, thresh=(0, 255)):
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
s_channel = hls[:,:,2]
binary_output = np.zeros_like(s_channel)
binary_output[(s_channel > thresh[0]) & (s_channel <= thresh[1])] = 1
return binary_output
def hls_select2(img, thresh=(0, 255)):
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
s_channel = hls[:,:,0]
binary_output = np.zeros_like(s_channel)
binary_output[(s_channel > thresh[0]) & (s_channel <= thresh[1])] = 1
return binary_output
# The Sobel Threshold
def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Apply x or y gradient with the OpenCV Sobel() function
# and take the absolute value
if orient == 'x':
abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 1, 0,ksize=sobel_kernel))
if orient == 'y':
abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 0, 1,ksize=sobel_kernel))
# Rescale back to 8 bit integer
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
# Create a copy and apply the threshold
grad_binary = np.zeros_like(scaled_sobel)
# Here I'm using inclusive (>=, <=) thresholds, but exclusive is ok too
grad_binary[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[0])] = 1
# Return the result
return grad_binary
# Magnitude of the gradient
def mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255)):
# Calculate gradient magnitude
# Apply threshold
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Take both Sobel x and y gradients
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Calculate the gradient magnitude
gradmag = np.sqrt(sobelx**2 + sobely**2)
# Rescale to 8 bit
scale_factor = np.max(gradmag)/255
gradmag = (gradmag/scale_factor).astype(np.uint8)
# Create a binary image of ones where threshold is met, zeros otherwise
mag_binary = np.zeros_like(gradmag)
mag_binary[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1
return mag_binary
# Direction of the Gradient threshold
def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):
# Calculate gradient direction
# Grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Calculate the x and y gradients
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Take the absolute value of the gradient direction,
# apply a threshold, and create a binary image result
absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
dir_binary = np.zeros_like(absgraddir)
dir_binary[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1
# Return the binary image
return dir_binary
###
def select_yellow(image):
hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
lower = np.array([20,60,60])
upper = np.array([38,174, 250])
mask = cv2.inRange(hsv, lower, upper)
return mask
def select_white(image):
lower = np.array([202,202,202])
upper = np.array([255,255,255])
mask = cv2.inRange(image, lower, upper)
return mask
### COMBINED THRESHOLD
# This function combines the previous types of thresholds
def My_threshold(warped_im, ksize=3, Plot=False):
R = warped_im[:,:,0]
thresh = (220, 255)
binary = np.zeros_like(R)
binary[(R > thresh[0]) & (R <= thresh[1])] = 1
# Apply each of the thresholding functions
gradx = abs_sobel_thresh(warped_im, orient='x', sobel_kernel=ksize, thresh=(10, 255))
#hls_binary = hls_select(warped_im, thresh=(120, 255))
#grady = abs_sobel_thresh(warped_im, orient='y', sobel_kernel=3, thresh=(0, 255))
#mag_binary = mag_thresh(warped_im, sobel_kernel=7, mag_thresh=(20, 255))
#dir_binary = dir_threshold(warped_im, sobel_kernel=ksize, thresh=(1.0,1.5))
"""
# Combine Thresholds
combined = np.zeros_like(gradx)
combined[(gradx == 1) ] = 1
combined[(mag_binary == 1)] = 1
combined[(grady == 1)] = 0
combined[(binary == 1)] = 1
"""
# Apply each of the thresholding functions
yellow = select_yellow(warped_im)
white = select_white(warped_im)
# Combine Thresholds
combined = np.zeros_like(yellow)
combined[(binary == 1)] = 1
combined[(gradx == 1) ] = 1
combined[(yellow >= 1) ] = 1
combined[(white >= 1)] = 1
if Plot == True:
histogram = np.sum(combined[combined.shape[0]//2:,:], axis=0)
f, (ax1,ax2) = plt.subplots(1,2, figsize = (20,10))
ax1.set_title('Binary Image')
ax1.imshow(combined, cmap='gray')
ax2.set_title('Histogram')
ax2.plot(histogram)
return combined
### PERSPECTIVE TRANSFORM
def warp(img,A,B,C,D):
img_size = (img.shape[1], img.shape[0])
src = np.float32([A,B,C,D])
#dst = np.float32([[350, 50],[350, B[1]], [950,C[1]], [950, 50]])
dst = np.float32([[B[0]-30, 50],[B[0]-30, B[1]], [C[0]+30,C[1]], [C[0]+30, 50]])
# Compute the perspective transform, M
M = cv2.getPerspectiveTransform(src,dst)
# Inverse
Minv = cv2.getPerspectiveTransform(dst,src)
warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
return warped, Minv
### FINDING lINES
def finding_the_Lines(undist_image, binary_warped, Minv,
first_frame=True,
previous_lane_image=None,
previous_lane_detected=False,
previous_left_fit = None,
previous_right_fit=None,
plot=False):
# undist_image: Undistorted image
# binary_warped: Binary warped image
# first_frame: If the frame is the first generated (True or False)
# previous_lane_image: The previous inverse warped image with the lane drawn
# previous_lane_detected: If the frame the previous lane was detected (True or False)
# previous_left_fit: Previous left fit array to not blind search
# previous_right_fit: Previous left fit array to not blind search
# plot: Plot the images fo each step (True of False)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Set the width of the windows +/- margin
margin = 130
newwarp = previous_lane_image
# Create an image to draw on and an image to show the selection window
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
window_img = np.zeros_like(out_img)
if ( (first_frame == True) | (previous_lane_detected == False) ):
# Take a histogram of the bottom half of the image
histogram = np.sum(binary_warped[int(binary_warped.shape[0]/2):,:], axis=0)
# Create an output image to draw on and visualize the result
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]/2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# Choose the number of sliding windows
nwindows = 9
# Set height of windows
window_height = np.int(binary_warped.shape[0]/nwindows)
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Set minimum number of pixels found to recenter window
minpix = 50
##########
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2)
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# IF LANE FAIL
if ((len(lefty) == 0) | (len(leftx) == 0 ) | (len(rightx) == 0 ) | (len(righty) == 0 )):
detected = False
# Combine the result with the original image
result3 = cv2.addWeighted(undist_image, 1, previous_lane_image, 0.3, 0)
left_fit = None
right_fit = None
else:
detected = True
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
else:
left_fit = previous_left_fit
right_fit = previous_right_fit
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] + margin)))
# Again, extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
if ((len(lefty) == 0) | (len(leftx) == 0 ) | (len(rightx) == 0 ) | (len(righty) == 0 )):
detected = False
# Combine the result with the original image
result3 = cv2.addWeighted(undist_image, 1, previous_lane_image, 0.3, 0)
left_fit = None
right_fit = None
else:
detected = True
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
# Generate x and y values for plotting
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
if detected == True:
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
### Final Image - Drawing
# Create an image to draw the lines on
warp_zero = np.zeros_like(binary_warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
# Check number of pixels of each lane
i = 0
ii = 0
for j in nonzero[1]:
if j < 400: # nonzero pixels before x = 400
i+=1
if j > 800: # nonzero pixels after x = 800
ii+=1
# If the non zeros pixels are few points in one of the two lines
if ((i < 1500) | (ii < 1500)):
detected = False # force blind search next time
left_fit = None
right_fit = None
# If the nonzeros pixels are very few points
if (((i < 700) & (i > 45000)) | ((ii < 700) & (ii > 45000))):
result3 = cv2.addWeighted(undist_image, 1, previous_lane_image, 0.3, 0)
else:
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0]))
### FINAL IMAGE IF detected = True
# Combine the result with the original image
result3 = cv2.addWeighted(undist_image, 1, newwarp, 0.3, 0)
# Plot 3 images
if plot == True:
f, (ax1,ax2,ax3) = plt.subplots(1,3, figsize = (16,8))
### Figure 1
ax1.imshow(out_img)
ax1.plot(left_fitx, ploty, color='yellow')
ax1.plot(right_fitx, ploty, color='yellow')
ax1.set_xlim([0, 1280])
ax1.set_ylim([720, 0])
# Create an image to draw on and an image to show the selection window
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
window_img = np.zeros_like(out_img)
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
result1 = out_img
ax1.set_title('Finding the Lines')
ax1.imshow(result1)
###
### Figure 2
# Create an image to draw on and an image to show the selection window
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
window_img = np.zeros_like(out_img)
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
result2 = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
ax2.set_title('Draw the finding Lines')
ax2.imshow(result2)
ax2.plot(left_fitx, ploty, color='yellow')
ax2.plot(right_fitx, ploty, color='yellow')
ax2.set_xlim([0, 1280])
ax2.set_ylim([720, 0])
###
# Figure 3
ax3.set_title('Final result')
ax3.imshow(result3)
final_processed_frame = result3
xm_per_pix = 3.7/700
if len(lefty) > 1 and len(righty) > 1:
lane_offset = (640 - (left_fitx[-1]+right_fitx[-1])/2)*xm_per_pix
else:
lane_offset = 0
return final_processed_frame, newwarp, lane_offset, detected, left_fit, right_fit, left_fitx, right_fitx, ploty
### CURVATURE MEASURE
def measure_curve(leftx,rightx,ploty):
# y_eval is in bottom of the image
y_eval = np.max(ploty)
bottom_index = len(ploty) - 1
#Convert x,y pixels to meters
ym_per_pix = 30/len(ploty)#meters per pixel in y dimension
lane_width = np.absolute(leftx[bottom_index] - rightx[bottom_index])
xm_per_pix = 3.7/lane_width #meters per pixel in x dimension
#Fit new polynominals to x,y
left_fit_new = np.polyfit(ploty*ym_per_pix, leftx*xm_per_pix, 2)
right_fit_new = np.polyfit(ploty*ym_per_pix, rightx*xm_per_pix, 2)
#Calculate new radius of curvature
left_curverad = ((1 + (2*left_fit_new[0]*y_eval*ym_per_pix + left_fit_new[1])**2)**1.5) / np.absolute(2*left_fit_new[0])
right_curverad = ((1 + (2*right_fit_new[0]*y_eval*ym_per_pix + right_fit_new[1])**2)**1.5) / np.absolute(2*right_fit_new[0])
#print radius of curvature in meters
curverad = (left_curverad, right_curverad)
return curverad
### TEST 1
#reading an image
image = mpimg.imread('test_images/straight_lines1.jpg')
#printing out
print('This image is:', type(image), 'with dimensions:', image.shape)
plt.imshow(image)
# undistortion image
undist1, mtx1, dist1 = cal_undistort(image, objpoints, imgpoints)
plt.imshow(undist1)
### Area adjustment
imshape = undist1.shape
image_copy = np.copy(undist1)
# VERTICES
# A D
# B C
"""
A = [580, 460]
B = [275, imshape[0] - 50]
C = [imshape[1]-240,imshape[0] - 50]
D = [imshape[1] - 580, 460]
"""
A = [595, 450]
B = [275, imshape[0] - 50]
C = [imshape[1]-240,imshape[0] - 50]
D = [imshape[1] - 590, 450]
vertices = np.array([[A, B, C, D]], dtype=np.int32)
image_draw = cv2.polylines(image_copy, vertices, 255, [0,0,200], thickness=3, lineType=4)
plt.imshow(image_draw)
warped_im1, Minv1 = warp(undist1,A,B,C,D)
plt.imshow(warped_im1)
binary_warped1 = My_threshold(warped_im1, ksize=3, Plot=True)
Test_result = finding_the_Lines(undist1, binary_warped1, Minv1, plot=True)
print (Test_result[2])
plt.imshow(Test_result[0])
### OFFSET AND CURVATURE
# OFFSET
print ("Lane offset: ", Test_result[2])
print (" ")
# CURVATURE
curverad = measure_curve(Test_result[-3],Test_result[-2],Test_result[-1])
print ('Radius of curvature is in meters: ')
print ('Left Line: ', curverad[0], 'm')
print ('Right Line: ', curverad[1], 'm')
#str1 = 'Left Line: ' + str(curverad[0]) + 'm'
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(Test_result[2]) + ' m'
output_image_name = 'test_images_output/' + 'straight_lines1' + '_output' + '.jpg'
im = Image.fromarray(Test_result[0])
im.save(output_image_name)
img = Image.open(output_image_name)
font_type = ImageFont.truetype("Arial.ttf",50)
drawn = ImageDraw.Draw(img)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(530,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
plt.imshow(img)
### TEST 2
#reading an image
image2 = mpimg.imread('test_images/test5.jpg')
plt.imshow(image2)
# Undistorted image 2
undist2, mtx2, dist2 = cal_undistort(image2, objpoints, imgpoints)
plt.imshow(undist2)
image_copy2 = np.copy(undist2)
image_draw2 = cv2.polylines(image_copy2, vertices, 255, [0,0,200], thickness=3, lineType=4)
plt.imshow(image_draw2)
warped_im2, Minv2 = warp(undist2,A,B,C,D)
plt.imshow(warped_im2)
binary_warped2 = My_threshold(warped_im2, ksize=3, Plot=True)
Test2_result = finding_the_Lines(undist2, binary_warped2, Minv2, plot=True)
### OFFSET AND CURVATURE
# OFFSET
print ("Lane offset: ", Test2_result[2])
print ("")
# CURVATURE
curverad2 = measure_curve(Test2_result[-3],Test2_result[-2],Test2_result[-1])
print ('Radius of curvature in meters (m): ')
print ('Left Line: ', curverad2[0], 'm')
print ('Right Line: ', curverad2[1], 'm')### TEST 2
### TEST 3
#reading an image
image3 = mpimg.imread('input_images/image990.jpg')
undist3, mtx3, dist3 = cal_undistort(image3, objpoints, imgpoints)
warped_im3, Minv3 = warp(undist3,A,B,C,D)
binary_warped3 = My_threshold(warped_im3, ksize=3, Plot=False)
Test3_result = finding_the_Lines(undist3, binary_warped3, Minv3, plot=True)
binary_warped3 = My_threshold(warped_im3, ksize=3, Plot=True)
### ALL TEST IMAGES
straight1 = mpimg.imread('test_images/straight_lines1.jpg')
straight2 = mpimg.imread('test_images/straight_lines2.jpg')
test1 = mpimg.imread('test_images/test1.jpg')
test2 = mpimg.imread('test_images/test2.jpg')
test3 = mpimg.imread('test_images/test3.jpg')
test4 = mpimg.imread('test_images/test4.jpg')
test5 = mpimg.imread('test_images/test5.jpg')
test6 = mpimg.imread('test_images/test6.jpg')
#
straight1_undist, straight1_mtx, straight1_dist = cal_undistort(straight1, objpoints, imgpoints)
straight2_undist, straight2_mtx, straight2_dist = cal_undistort(straight2, objpoints, imgpoints)
test1_undist, test1_mtx, test1_dist = cal_undistort(test1, objpoints, imgpoints)
test2_undist, test2_mtx, test2_dist = cal_undistort(test2, objpoints, imgpoints)
test3_undist, test3_mtx, test3_dist = cal_undistort(test3, objpoints, imgpoints)
test4_undist, test4_mtx, test4_dist = cal_undistort(test4, objpoints, imgpoints)
test5_undist, test5_mtx, test5_dist = cal_undistort(test5, objpoints, imgpoints)
test6_undist, test6_mtx, test6_dist = cal_undistort(test6, objpoints, imgpoints)
#
warped_straight1, Minv = warp(straight1_undist,A,B,C,D)
warped_straight1, Minv = warp(straight2_undist,A,B,C,D)
warped_test1, Minv = warp(test1_undist,A,B,C,D)
warped_test2, Minv = warp(test2_undist,A,B,C,D)
warped_test3, Minv = warp(test3_undist,A,B,C,D)
warped_test4, Minv = warp(test4_undist,A,B,C,D)
warped_test5, Minv = warp(test5_undist,A,B,C,D)
warped_test6, Minv = warp(test6_undist,A,B,C,D)
#
binary_warp_straight1 = My_threshold(warped_straight1, ksize=3, Plot=False)
binary_warp_straight2 = My_threshold(warped_straight1, ksize=3, Plot=False)
binary_warp_test1 = My_threshold(warped_test1, ksize=3, Plot=False)
binary_warp_test2 = My_threshold(warped_test2, ksize=3, Plot=False)
binary_warp_test3 = My_threshold(warped_test3, ksize=3, Plot=False)
binary_warp_test4 = My_threshold(warped_test4, ksize=3, Plot=False)
binary_warp_test5 = My_threshold(warped_test5, ksize=3, Plot=False)
binary_warp_test6 = My_threshold(warped_test6, ksize=3, Plot=False)
#
result_straight1 = finding_the_Lines(straight1_undist, binary_warp_straight1,Minv, plot=False)
curverad = measure_curve(result_straight1[-3], result_straight1[-2], result_straight1[-1])
output_image_name = 'test_images_output/' + 'result_straight1.jpg'
im = Image.fromarray(result_straight1[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_straight1[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_straight2 = finding_the_Lines(straight2_undist, binary_warp_straight2,Minv, plot=False)
curverad = measure_curve(result_straight2[-3], result_straight2[-2], result_straight2[-1])
output_image_name = 'test_images_output/' + 'result_straight2.jpg'
im = Image.fromarray(result_straight2[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_straight2[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test1 = finding_the_Lines(test1_undist, binary_warp_test1, Minv, plot=False)
curverad = measure_curve(result_test1[-3], result_test1[-2], result_test1[-1])
output_image_name = 'test_images_output/' + 'result_test1.jpg'
im = Image.fromarray(result_test1[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_straight1[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test2 = finding_the_Lines(test2_undist, binary_warp_test2, Minv, plot=False)
curverad = measure_curve(result_test2[-3], result_test2[-2], result_test2[-1])
output_image_name = 'test_images_output/' + 'result_test2.jpg'
im = Image.fromarray(result_test2[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_test2[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test3 = finding_the_Lines(test3_undist, binary_warp_test3, Minv, plot=False)
curverad = measure_curve(result_test3[-3], result_test3[-2], result_test3[-1])
output_image_name = 'test_images_output/' + 'result_test3.jpg'
im = Image.fromarray(result_test3[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_test3[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test4 = finding_the_Lines(test4_undist, binary_warp_test4, Minv, plot=False)
curverad = measure_curve(result_test4[-3], result_test4[-2], result_test4[-1])
output_image_name = 'test_images_output/' + 'result_test4.jpg'
im = Image.fromarray(result_test4[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_test4[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test5 = finding_the_Lines(test5_undist, binary_warp_test5, Minv, plot=False)
curverad = measure_curve(result_test5[-3], result_test5[-2], result_test5[-1])
output_image_name = 'test_images_output/' + 'result_test5.jpg'
im = Image.fromarray(result_test5[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_test5[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
result_test6= finding_the_Lines(test6_undist, binary_warp_test6, Minv, plot=False)
curverad = measure_curve(result_test6[-3], result_test6[-2], result_test6[-1])
output_image_name = 'test_images_output/' + 'result_test6.jpg'
im = Image.fromarray(result_test6[0])
im.save(output_image_name)
im = Image.open(output_image_name)
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(result_test6[2]) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(670,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
### SAVE THE FRAMESIN THE FOLDER 'input_images/'
#pipeline_images = video_to_frames('project_video.mp4', 'input_images/')
### PROCESS ALL IMAGES
# Previous lane image
previous_left_fit = None
previous_right_fit = None
previous_lane_image = None
font_type = ImageFont.truetype("Arial.ttf",40)
input_images = []
count = 0
for i in range(1257):
image_name = 'input_images/image' + str(count) + '.jpg'
input_images.append(image_name)
#print (image_name)
count = count + 1
count = 0
for fname in input_images:
# Read the frame
frame = mpimg.imread(fname) # array image
# Undistorted image
undist, mtx, dist = cal_undistort(frame, objpoints, imgpoints)
# Perspective Transform
warped_im, Minv = warp(undist,A,B,C,D)
# Compute binary undistorted images and save it
binary_undist = My_threshold(undist, ksize=3)
#binary_undist_name = 'binary_warped_images/' + 'binary_undist' + str(count) + '.jpg'
#im = Image.fromarray(binary_warped)
#im.save(binary_undist_name)
# Compute binary warped and undistorted images and save it
binary_warped = My_threshold(warped_im, ksize=3)
#binary_warped_name = 'binary_warped_images/' + 'binary_warped' + str(count) + '.jpg'
#im = Image.fromarray(binary_warped)
#im.save(binary_warped_name)
# Second, third, ... last frame
if count != 0:
final_image, lane_image, lane_offset, detected, left_fit, right_fit, left_fitx, right_fitx, ploty = finding_the_Lines(undist, binary_warped, Minv,
first_frame=False,
previous_lane_image=previous_lane_image,
previous_lane_detected=previous_lane_detected,
previous_left_fit = previous_left_fit,
previous_right_fit=previous_right_fit,
plot=False)
else:
# First Frame
final_image, lane_image, lane_offset, detected, left_fit, right_fit,left_fitx, right_fitx, ploty = finding_the_Lines(undist, binary_warped, Minv,
first_frame=True,
previous_lane_image=previous_lane_image,
previous_lane_detected=False,
previous_left_fit = None,
previous_right_fit=None,
plot=False)
curverad = measure_curve(left_fitx, right_fitx, ploty)
previous_lane_image = lane_image
previous_left_fit = left_fit
previous_right_fit = right_fit
previous_lane_detected = detected
im = Image.fromarray(final_image)
output_image_name = 'output_images/' + 'image' + str(count) + '.jpg'
im.save(output_image_name)
im = Image.open(output_image_name)
# Write Lane offset and Curvature in the image
str1 = 'Left Line: ' + '{:.2f}'.format(curverad[0]) + ' m'
str2 = 'Right Line: ' + '{:.2f}'.format(curverad[1]) + ' m'
str3 = 'Lane offset: ' + '{:.2f}'.format(lane_offset) + ' m'
drawn = ImageDraw.Draw(im)
drawn.text(xy=(30,30),text=str1, fill=(255,255,255), font=font_type)
drawn.text(xy=(530,30),text=str2, fill=(255,255,255), font=font_type)
drawn.text(xy=(30,130),text=str3, fill=(255,255,255), font=font_type)
im.save(output_image_name)
count = count + 1
### MAKE A VIDEO
from moviepy.editor import VideoFileClip
from IPython.display import HTML
from moviepy.editor import ImageSequenceClip
output_images = []
count = 0
for i in range(1257):
image_name = 'output_images/image' + str(count) + '.jpg'
output_images.append(image_name)
count = count + 1
seq = ImageSequenceClip(output_images, fps=25)
video_file = 'myvideo' + '.mp4'
seq.write_videofile(video_file)
HTML("""
<video width="960" height="540" controls>
<source src="myvideo.mp4" type="video/mp4">
</video>
""")